#include <unistd.h>
#include <sstream>
#include <iostream>

#include "CmdHandler.h"
#include "SerCom.h"

std::string CmdHandler::readBatteryVoltage()
{
    return execCommand( 'a' );
}


std::string CmdHandler::readRaspiVoltage()
{
    return execCommand( 'b' );

}


std::string CmdHandler::readHubVoltage()
{
    return execCommand( 'c' );
}


bool CmdHandler::isCharging()
{
    std::string result = execCommand( 'd' );
    return result == "y" ? true : false;
}


CmdHandler::ChargingMode CmdHandler::readChargeMode()
{
    std::string result = execCommand( 'l' );
    if( result == "u" ) {
        return ChargingMode::Auto;
    }
    else if( result == "y" ) {
        return ChargingMode::Always;
    }
    return ChargingMode::Never;
}


void CmdHandler::setChargingModel( CmdHandler::ChargingMode mode )
{
    char cmd = 'u'; //Auto
    if( mode == ChargingMode::Always ) {
        cmd = 'y';
    }
    else if( mode == ChargingMode::Never ) {
        cmd = 'n';
    }
    execCommand( cmd, false );
}


std::string CmdHandler::execCommand( char cmd, bool waitForResponse )
{
    SerCom::get()->write( &cmd, 1 );
    usleep( 100 );
    if( waitForResponse ) {
        std::string resp = SerCom::get()->read();
        resp = resp.substr( resp.find_first_of( "#" ) + 1 );
        std::stringstream trimmer;
        trimmer << resp;
        resp.clear();
        trimmer >> resp;
        return resp;
    }
    return std::string( "" );
}
